/**
 * 
 */
package com.intervalstudios.wiimotegrabber;

import com.sun.corba.se.impl.ior.WireObjectKeyTemplate;

import wiiremotej.IRLight;
import wiiremotej.event.WRIREvent;

/**
 * @author josh adapted from code by carl kenner: http://www.wiili.org/index.php/GlovePIE:Carl%27s_IR_Mouse
 *
 */
public class MouseConverter {

	/**
	 * converts IR data into two points,  X and Y:
	 */
	public static long startTime = System.currentTimeMillis();
	public static double [] convertIRtoXY(WRIREvent ir,double roll) {
		// TODO Auto-generated constructor stub
		double mx =0;
		double my = 0;
		
		
		IRLight[] lights = ir.getIRLights();
		//  
		/*
		for(int i = 0;i<lights.length;i++){
			if(lights[i]!=null){ 
				System.out.println("lights "+i+" x="+ lights[i].getX()+" y="+lights[i].getY());
			}else{
				System.out.println("light "+i+ " = null");
			}
		}
		//*/
		// if(lights[0])
		
//		double c = Math.cos(roll);
//		double s = Math.sin(roll);
//		
//		// Change these values:
//		double SensorBarSeparation = 7.5;// inches  // distance between middles of two sensor bar dots
////		boolean NoYawAllowed = true  // Calculates X if no yaw is allowed, otherwise calculates Yaw but not X
//		double IRMulX = 1.2;
//		double IRMulY = 1.2;
//		double IROffsetX = 0;  // add to mouse.x
//		double IROffsetY = 0;  // add to mouse.y
//		// /*
//		double dot1x=0;
//		double dot1y=0;
//		double dot2x=0;
//		double dot2y=0;
//		// Compensate for roll
//		if(lights[0]!=null){
//			dot1x = c*(511.5-lights[0].getX())/511.5 - s*(lights[0].getY()-383.5)/511.5;
//			dot1y = s*(511.5-lights[0].getX())/511.5 + c*(lights[0].getY()-383.5)/511.5;
//		}
//		if(lights[1]!=null){
//			dot2x = c*(511.5-lights[1].getX())/511.5 - s*(lights[1].getY()-383.5)/511.5;
//			dot2y = s*(511.5-lights[1].getX())/511.5 + c*(lights[1].getY()-383.5)/511.5;
//		}
		
		
//		System.out.println(ir.getSource().isIRSensorEnabled());
		
//				
		if(lights[0]!=null && lights[1]!=null){
			double dx = Math.abs(lights[0].getX()-lights[1].getX());
			double dy = Math.abs(lights[0].getY()-lights[1].getY());			
			double dist = Math.hypot(dx,dy);
			
			double xDistMod = 1/(1-dist);
			// xDistMod/100
			
			mx = (lights[0].getX()+lights[1].getX())/2;
			mx = 1-mx;
			
			// ZERO OUT MX:
			// double offs = mx;
			double offset = (1/(xDistMod))/2;
			
			mx=(mx*xDistMod)-(dist);
			mx*=1.2;
			//mx-=.05;
			// System.out.println("dist = "+dist+"                 &x="+mx+"          &mod="+xDistMod);
			// mx*=xDistMod;
			my = (lights[0].getY()+lights[1].getY())/2;
			my*=1.2;
			my-=.05;
//			System.out.println("y="+my);
						
		}else if(lights[0]==null && lights[1]==null && lights[2]==null && lights[3]==null){
			// System.out.println("not detecting lights-  trying to restart camera");
			if((startTime-System.currentTimeMillis())%10000==0){
//				System.out.println("trying to restart IR");
				try {
					// if(ir.getSource().isConnected()) ir.getSource().setIRSensorEnabled(true, WRIREvent.BASIC,WiiMoteGrabber.sensitivity1[WiiMoteGrabber.wiimoteSensitivity],WiiMoteGrabber.sensitivity2[WiiMoteGrabber.wiimoteSensitivity]);
				} catch (Exception e) {
					// TODO: handle exception
					System.out.println(e);
				}	
			}
		}
		
		/*
		// if both dots are visible check which is which and how far apart
		int leftdot;
		int rightdot;
		double IRDistance;
		double dotdeltax;
		double dotdeltay;
		
		
		if(lights[0]!=null && lights[1]!=null){
			if(dot1x<=dot2x){
				leftdot = 1;
			    dotdeltay = dot2y - dot1y;	
			}else{
				leftdot = 2;
			    dotdeltay = dot1y - dot2y;				
			}			
		  dotdeltax = Math.abs(dot1x-dot2x);
		  double DotSep = Math.hypot(dotdeltax, dotdeltay) * 511.5;
		  IRDistance = SensorBarSeparation * 1320 / DotSep;
		}
		// sort out the position of the left and right dots
		double LeftDotX;
		double LeftDotY;
		boolean LeftDotVis;
		double RightDotX;
		double RightDotY;
		boolean RightDotVis;
		
		if( leftdot == 1){
		  if (lights[0]!=null && lights[1]!=null){
		    LeftDotX = dot1x;
		    LeftDotY = dot1y;
		    LeftDotVis = true;
		    RightDotX = dot2x;
		    RightDotY = dot2y;
		    RightDotVis = true;
		  }else if(lights[0]!=null){
		    if (Math.hypot(leftdotx-dot1x,leftdoty-dot1y) <= Math.hypot(rightdotx-dot1x,rightdoty-dot1y){
		      // is the real dot 1
		      LeftDotX = dot1x;
		      LeftDotY = dot1y;
		      RightDotX = dot1x + dotdeltax;
		      RightDotY = dot1y + dotdeltay;
		      LeftDotVis = true;
		      RightDotVis = false;
		    }else{
		      // was originally dot 2, but now called dot 1.
		      leftdot = 2 // this dot (1) is actually the right dot
		      LeftDotX = dot1x - dotdeltax;
		      LeftDotY = dot1y - dotdeltay;
		      RightDotX = dot1x;
		      RightDotY = dot1y;
		      RightDotVis = true;
		      LeftDotVis = false;
		    }
		  }else if (lights[1]!=null){
		    LeftDotX = dot2x - dotdeltax;
		    LeftDotY = dot2y - dotdeltay;
		    RightDotX = dot2x;
		    RightDotY = dot2y;
		    RightDotVis = true;
		    LeftDotVis = false;
		  }
		}else if(leftdot == 2){		
		  if (lights[0]!=null && lights[1]!=null){
		    LeftDotX = dot2x
		    LeftDotY = dot2y
		    LeftDotVis = true
		    RightDotX = dot1x
		    RightDotY = dot1y
		    RightDotVis = true
		  }else if (lights[0]!=null){
		    if(hypot(leftdotx-dot1x,leftdoty-dot1y) <= hypot(rightdotx-dot1x,rightdoty-dot1y)){
		      leftdot = 1 // dot 1 is now the left dot
		      LeftDotX = dot1x
		      LeftDotY = dot1y
		      RightDotX = dot1x + dotdeltax
		      RightDotY = dot1y + dotdeltay
		      LeftDotVis = true
		      RightDotVis = false
		    }else{
		      // the real dot 1 (on the right)
		      LeftDotX = dot1x - dotdeltax
		      LeftDotY = dot1y - dotdeltay
		      RightDotX = dot1x
		      RightDotY = dot1y
		      RightDotVis = true
		      LeftDotVis = false
		  	}
		  }else if (lights[1]!=null){
		    RightDotX = dot2x + dotdeltax
		    RightDotY = dot2y + dotdeltay
		    LeftDotX = dot2x
		    LeftDotY = dot2y
		    LeftDotVis = true
		    RightDotVis = false
		  }
		}else{
		
		  LeftDotX = dot1x;
		  LeftDotY = dot1y;
		  RightDotX = LeftDotX;
		  RightDotY = LeftDotY;
		  LeftDotVis = true;
		  RightDotVis = true;		
		}

		// Find the imaginary middle dot
		MiddleDotX = (leftdotx + rightdotx)/2;
		MiddleDotY = (leftdoty + rightdoty)/2;
		MiddleDotVis = lights[0]!=null or lights[1]!=null;

		if MiddleDotVis then
		  TotalPitch = atan2(511.5*MiddleDotY,1320) + Wiimote.Pitch
		  DotYaw = atan2(-511.5*MiddleDotX,1320) // assume yaw is 0
		  WiimoteYawNoX = atan2(511.5*MiddleDotX,1320)
		  WiimoteXNoYaw = -sin(dotyaw)*IRDistance
		  WiimoteY = -sin(totalpitch)*IRDistance
		  WiimoteZ = (-sqrt(sqr(IRDistance) - sqr(WiimoteY)))*IRDistance/RemoveUnits(IRDistance)
		end if

		// scale it to the screen range 0 to 1
		IRx = IRMulX*middledotx/2 + 0.5
		IRy = IRMulY*middledoty*1023/767/2 + 0.5
		IRvis = lights[0]!=null or lights[1]!=null
		IROnScreen = 0 <= IRx <= 1  and  0 <= IRy <= 1

		// is it off the screen?
		IRTooFarLeft = IRx < 0 or (IRx < 0.1 and (not IRvis))
		IRTooFarRight = IRx > 1 or (IRx > 1-0.1 and (not IRvis))
		IRTooFarUp = IRy < 0 or (IRy < 0.1 and (not IRvis))
		IRTooFarDown = IRy > 1 or (IRy > 1-0.1 and (not IRvis))
		
		
		
		
		
		
		
		
		
		
		
		
		
		
		
		*/
		
		double[] mousePoint = new double[]{mx,my};
		return mousePoint;
	}

}
